#include "MPU6050.h"

int MPU6050_SelfTest()
{
    int result;
    long gyro[3],accel[3];

    result = mpu_run_self_test(gyro,accel);
    if(result == 0x3)
    {
        float sens;
        uint16_t accel_sens;
        mpu_get_gyro_sens(&sens);
        gyro[0] = (long)(gyro[0]*sens);
        gyro[1] = (long)(gyro[1] * sens);
		gyro[2] = (long)(gyro[2] * sens);
		dmp_set_gyro_bias(gyro);
		mpu_get_accel_sens(&accel_sens);
		accel[0] *= accel_sens;
		accel[1] *= accel_sens;
		accel[2] *= accel_sens;
		dmp_set_accel_bias(accel);
		return 0;
    }
    else return -1;
}

static signed char gyro_orientation[9] = {-1, 0, 0,
                                           0,-1, 0,
                                           0, 0, 1};


unsigned short inv_row_2_scale(const signed char *row)
{
    unsigned short b;

    if (row[0] > 0)
        b = 0;
    else if (row[0] < 0)
        b = 4;
    else if (row[1] > 0)
        b = 1;
    else if (row[1] < 0)
        b = 5;
    else if (row[2] > 0)
        b = 2;
    else if (row[2] < 0)
        b = 6;
    else
        b = 7;      // error
    return b;
}

unsigned short inv_orientation_matrix_to_scalar(
    const signed char *mtx)
{
    unsigned short scalar; 
    /*
       XYZ  010_001_000 Identity Matrix
       XZY  001_010_000
       YXZ  010_000_001
       YZX  000_010_001
       ZXY  001_000_010
       ZYX  000_001_010
     */

    scalar = inv_row_2_scale(mtx);
    scalar |= inv_row_2_scale(mtx + 3) << 3;
    scalar |= inv_row_2_scale(mtx + 6) << 6;


    return scalar;
}

int MPU6050_Init()
{
    int ret = 0;
    ret += mpu_init();
    ret += mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL);
    ret += mpu_configure_fifo(INV_XYZ_GYRO|INV_XYZ_ACCEL);
    ret += mpu_set_sample_rate(DEFAULT_MPU_HZ);
    
    // assert(mpu_reg_dump() == 0);
    ret += dmp_load_motion_driver_firmware();
    ret += dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));
    ret += dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT| DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_GYRO | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL);
    ret += dmp_set_fifo_rate(DEFAULT_MPU_HZ);
    ret += mpu_set_dmp_state(1);
    // 最后使能中断
    ret += dmp_set_interrupt_mode(DMP_INT_CONTINUOUS);
    if(ret != 0 ) return -1;
    else return 0;
}

int MPU6050_ReadAngle(float *yaw,float *roll,float *pinch)
{
    // 这里是读取角度的代码
    int16_t gyro[3],accel[3];
    int32_t quat[4]; uint32_t timestamp;
    int16_t sensors; uint8_t more;
    dmp_read_fifo(gyro,accel,quat,&timestamp,&sensors,&more);
    float q0,q1,q2,q3;
    if(sensors & INV_WXYZ_QUAT)
    {
        q0 = Q30_TO_FLOAT(quat[0]);
        q1 = Q30_TO_FLOAT(quat[1]);
        q2 = Q30_TO_FLOAT(quat[2]);
        q3 = Q30_TO_FLOAT(quat[3]);
        // 以弧度为单位
        *pinch  = asinf(-2*q1*q3+2*q0*q2);
        *roll   = atan2f(2*q2*q3+2*q0*q1, -2*q1*q1-2*q2*q2+1);
        *yaw    = atan2f(2*(q1*q2+q0*q3),q0*q0+q1*q1-q2*q2-q3*q3);
        return 0;
    }
    else
    {
        return -1;
    }
}